// Map datastructure by Marcus
// rev Nov 5, 9:33
#include <iostream>
#include <stdio.h>
#include <vector>
using namespace std;

//namespace //mzslam
//{

struct pos{
  int X;
  int Y;
};

//defines a cell in the grid
struct gridCell{
  float wallProbability;
  int tagIndex;
};

//define data about a tag
struct tagData{
  int tagID;
  char tagOrientation; //u: Undefined n: North s: South w: West e: East 
  pos position;
};

class Map {
  vector< vector<gridCell> > map;
  vector<tagData> tagList;
  int mapSizeX;
  int mapSizeY;
public:
  //initializer
  Map(){
    mapSizeX = -1;
    mapSizeY = -1;
  }
        
  //add cell will automatically call resize if the cell is out of the current bounds
  void addCell(int positionX, int positionY, int tagID,char tagOrientation , float wallProbability){
    if(positionX +1 > mapSizeX || positionY + 1 > mapSizeY){
      resizeMap(positionX + 1, positionY + 1);
    }
    cout << "resized" << endl;
    gridCell newGridCell = {0.5, -1};
    newGridCell.wallProbability = wallProbability;
    if(tagID != -1){
      tagData newTag = {0.5, -1, -1, -1};
      newTag.tagID = tagID;
      newTag.tagOrientation = tagOrientation;
      pos p = {positionX, positionY};                 
      newTag.pos = p;
      tagList.push_back(newTag);
      newGridCell.tagIndex = tagList.size() - 1;      
    }
    cout << "about to add cell" << endl;
    cout << "mapsizeY " << mapSizeY << "actualSizeY " << map.size() << endl; 
    cout << "mapsizeX " << mapSizeX << "actualSizeX " << map[0].size() << endl; 
    map[positionY][positionX] = newGridCell;
  }

  float getPFromCell(int xPosition, int yPosition){
    return map[yPosition][xPosition].wallProbability;
  }
        
  int getTagIdFromCell(int xPosition, int yPosition){
    int index =  map[yPosition][xPosition].tagIndex;
    return tagList[index].tagID;
  }

  char getOrientationFromCell(int xPosition, int yPosition){
    int index =  map[yPosition][xPosition].tagIndex;
    return tagList[index].tagOrientation;
  }

  pos getTagPosition(int tagID){
    pos p = {-1,-1};
    for(int i = 0; i < tagList.size(); i++){
      if(tagList[i].tagID = tagID){
	p.X = tagList[i].tagPositionX;
	p.Y = tagList[i].tagPositionY;
	break;
      }
    }
    return p;
  }

  void print(){
    for(int i = 0; i < map.size(); i++){
      for(int j = 0; j < map[i].size();j++){
	cout << " " << map[i][j].wallProbability;
      }
      cout << endl;
    }
  }


private:
        
        
  //fill the existing lines so all lines are always of thesame length
  //add new lines of the new length so that the matrix will hold the new cell
  void resizeMap(int newSizeX, int newSizeY){
    cout << "newSizeX, newSizeY" << newSizeX << " , " << newSizeY << endl; 
    if(newSizeX > mapSizeX){
      cout << "resizeX" << endl;
      for(int i = 0 ; i < mapSizeY;i++){
	while(map[i].size() != newSizeX){
	  gridCell g = {0.5, -1};
	  map[i].push_back(g);
	}
      }
      mapSizeX = newSizeX;
    }
    if(newSizeY > mapSizeY){
      cout << "resize Y" << endl;
      while(map.size() != newSizeY){
	vector<gridCell> line;
	while(line.size() != mapSizeX){
	  gridCell g = {0.5, -1};
	  line.push_back(g);
	}
	map.push_back(line);    
      }
      mapSizeY = newSizeY;
    }
  }
};

//} // end of namespace
#if 0
int main( int argc, const char* argv[] )
{
  Map m;
  m.addCell(0,1,-1, 'u', 0.7);
  m.print();
  m.addCell(6,4,-1, 'u', 0.9);
  m.print();
}
#endif
